package edu.wpi.first.wpilibj.swarm;

import edu.wpi.first.wpilibj.Utility;

public class MovePD {
	double error;
	double dt;
	long   time;
	double tgtrate;
	double pwrdtgt;
	
	//static private final double Kp = 0.2;
	static private final double Kp = 0.05;
	//static private final double Kd = 0.02;
	static private final double Kd = 0.007;
	
	//static private final double velmult = 100.0;
	static private final double velmult = 122.0;
	
	public MovePD() {
		error   = 0.0;
		dt      = 0.0;
		tgtrate = 0.0;
		pwrdtgt = 0.0;
	}
	
	private void revPD(double vel) {
		pwrdtgt = (Kd * vel - Kp * error) / (Kp*dt + Kd);
	}
	
	private double PD(double vel, double pwr) {
		error += tgtrate * dt;
		return Kp * error + Kd * (tgtrate - vel) + pwr;
	}
	
	double control(double err, double vel, double pwr) {
		dt    = ((double) (Utility.getFPGATime() - time)) / 1000000.0;
		time += dt * 1000000.0;
		error = err;
		revPD(vel);
		tgtrate = pwr*velmult*(1.0 - Math.abs(pwr)) + pwrdtgt* Math.abs(pwr);
		return PD(vel, pwr);
	}
	double getErr() {
		return error;
	}
}